% Monte Carlo Localisation algorithm
% for the project of public transprot monitor
% MCL main function of the program
function MCL
% Setting up configuration
close all; clear all;hold on;

% Prepare necessary parameters for the model. Initialising data model.
% define the number of particles
global particleSize;
particleSize = 100;
% The time interval of reading GPS data,e.g.10 means every 10 seconds get a
% GPS reading. The minimum is 1.
global interval;
interval = 10;
% gps data log
global gps_data;
raw_gps_data = importdata('GPSLog.txt', ' ', 0);
gps_data = raw_gps_data.data;
gps_data = [gps_data(:,2) gps_data(:,1)];

% the number of movement steps, every step gets one reading
nSteps = 200;
% get the data of two bus routes
orig_route_map_39a = importdata('route_39a_oneway.txt', ' ', 0);
orig_route_map_145 = importdata('route_145_oneway.txt', ' ', 0);
orig_route_maps{1} = orig_route_map_39a;
orig_route_maps{2} = orig_route_map_145;

%% plot the graph of bus route map
% plot the bus routes based on bus stops
plotMap(orig_route_maps);
gps_39a_stops = importdata('GPSLog_for_stops.txt', ' ', 0);
global gps_stops;
gps_stops = [gps_39a_stops.data(:,2), gps_39a_stops.data(:,1)];
plot(gps_stops(:,1),gps_stops(:,2),'o');

%% Initialisation phase
% convert the coordinates of raw map to one-dimention distance
route_map_particle = convertMaps(orig_route_maps);
% generate particles over the entire map space
particles = generateParticles(route_map_particle);
% set the weights of particles to uniform distribution
n = length(particles);
p_w = cell(n,1);
total = sum(cellfun('length', particles));
for i = 1:n
    p_w{i} = ones(length(particles{i}),1)/total;
end
% set the control data, velocity
velocity = getVelocities();

%% recursive MCL algorithm starts
for k = 1:nSteps
    %%%%%%%%%%%%%%%%%%
    % prediction phase
    [particles, p_w] = estParticles(particles, p_w, velocity);
    %%%%%%%%%%%%%%%%%%
    % update phase
    % get a GPS measurement
    gps = getGPSReading(k);
    % calculate the weight for each particle based on GPS measurement
    if ~isempty(gps)
    [p_w,distances] = calcWeights(particles, gps, route_map_particle);
    end
    %%%%%%%%%%%%%%%%%%
    % resampling phase
    % remove the particles whose weight is zero
    [particles, p_w] = resample(particles, p_w);
    % draw the positon of particles on the map
    plotWeightedParticles(particles, p_w, route_map_particle);
end
end

%% functions for velocity
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimatie the velocities of the bus
function [velocity] = getVelocities()
    velocity = calculateVelocity(15);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% update the velocities of the transport based on observed velocity
% 1 m/s = 3.6 km/h; 100 km/h = 28 m/s;
function [velocity] = calculateVelocity(v)
% v_value = [0,10,20,30,40,50,60,70,80,90,100]';
v_value = [0,5,10,15,20,25,30]';
if v < 0
    % initialisation
    v_weight = ones(length(v_value), 1)/length(v_value);
else
    mu = v;
    sigma = 10;
    v_weight = normpdf(v_value, mu, sigma);
    v_weight = v_weight/sum(v_weight);
end
velocity = [v_value v_weight];
end

%% functions for resampling
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% resampling, remove the particles whose weight is zero
function [particles, p_w] = resample(particles, p_w)
n = length(particles);
for i=1:n
    min_weight = 0;
    t_p = particles{i}(p_w{i}>min_weight);
    t_w = p_w{i}(p_w{i} > min_weight);
    [particles{i}, p_w{i}] = reduceSize(t_p, t_w);
end
% normalisation
ls = cellfun('length', p_w);
W = cell2mat(p_w);
W = W/sum(W);
% return the particles to original format
p_w = mat2cell(W,ls);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% reduce particle size
function [new_particles, new_weights] = reduceSize(particles, weights)
global particleSize;
% sum(weights)
p_max = max(max(particles));
p_min = min(min(particles));
p_interval = (p_max - p_min)/particleSize;
new_particles = zeros(particleSize,1);
new_weights = zeros(particleSize,1);
j = 1;
for i = 1:particleSize
    low = p_min + (i-1)*p_interval;
    high = p_min + i*p_interval;
    index = (particles >= low)&(particles <= high);
    [w_max,idx] = max(weights(index));
    if idx
        tmp = particles(index);
        new_particles(j) = tmp(idx);
        new_weights(j) = sum(weights(index));
        j = j+1;
    end
end
end

%% fuctions for prediction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% estimation for multiple routes
function [particles, p_w] = estParticles(particles, p_w, velocity)
n = length(particles);
for i = 1:n
    p_wt = p_w{i};
    particle = particles{i};
    [estParticles, ep_w] = estSingleRouteParticles(particle, p_wt, velocity);
    p_w{i} = ep_w;
    particles{i} = estParticles;
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% predict particles based on velocity
% for each possible velocity, estimate the next particles
% return the weighted mean of the estimated particles
function [ep, p_wt] = estSingleRouteParticles(particle, p_wt, velocity)
% global route_map_particle;
global interval;
% time intervals for updating
dt = 1 * interval;
m = length(particle);
n = length(velocity);
% weight for particles based on different velocities
p_wt = p_wt * velocity(:,2)';
%sum(sum(p_wt))
% the possible distances with different velocities
tmp_d = zeros(m, n);
% estimate distances based on different velocities
% (distances-by-velocityies matrix)
for i = 1:n
    tmp_d(:,i) =  particle(:,1) + ones(m,1).* 0.001 * velocity(i,1) * dt;
end
[ep, p_wt] = mergeParticles(tmp_d, p_wt);
end

%% fuctions for measuremeny update
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate weight for each particles on multiple routes
function [weights,distances] = calcWeights(particles, gps, route_map_particle)
n = length(particles);
weights = cell(n,1);

for i = 1:n
    weights{i} = calcWeight(particles{i}, gps, route_map_particle{i});
end

distances = weights;
% remember the length of the weight for particles on each route
ls = cellfun('length', weights);
% weights are the distances between the GPS data and the particles
% assuming the distance between the best particle and the perfect GPS data
% is zero, which means the GPS measurement is perfect.
mu = 0;
% the noise of the distance value
sigma = 0.01;
% all the particles in normal distribution
W = cell2mat(weights);
W = normpdf(W, mu, sigma);
W = W/sum(W);
% return the particles to original format
weights = mat2cell(W,ls);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% calculate difference for each particles comparing to GPS
function [weights] = calcWeight(particles, gps, refRoute)
% routeL containing Latitude and Longitude
% routeL compare with gps reading to get weights
routeL = convertParticles(particles, refRoute);
len = length(routeL);
X = zeros(len,1);
for i = 1:len
%     X(i,1) = sqrt((routeL(i,1) - gps(1)).^2 + (routeL(i,2) - gps(2)).^2);
    X(i,1) = haversine([routeL(i,2) routeL(i,1)],[gps(2) gps(1)]);
end
weights = X;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% remove the same particles, sum the weights of the same particles together
function [new_particles, new_weights] = mergeParticles(particles, weights)
[m,n] = size(particles);
new_particles = [];
new_weights = [];
for i = 1:m*n
    index = find(new_particles == particles(i));
    if index
        new_weights(index) = new_weights(index) + weights(i);
    else
        new_particles(length(new_particles)+1) = particles(i);
        new_weights(length(new_weights)+1) = weights(i);
    end
end
new_particles = new_particles';
new_weights = new_weights';
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% convert distance to positions
function [position] = convertParticles(particles, refRoute)
n = length(particles);
position = zeros(n,2);
for i = 1:n
    position(i,:) = convertDistance(particles(i), refRoute);
end
end

%% fuctions for initialisation
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% generate more particles based on original particles of the routes
% simply try dividing the route equally
function [particles] = generateParticles(routes)
n = length(routes);
particles = cell(n,1);
for i=1:n
    particles{i} = initialiseParticles(routes{i});
end
end

% generate more particles based on original particles of the routes
% simply try dividing the route equally
function [particles] = initialiseParticles(bus_route)
% distance between particles, assuming the distance is 10 meters
dp = 0.01;
total_length = max(bus_route(:,2));
% total number of particles on a route
np = fix(total_length/dp);
particles = zeros(np,1);
for i = 2:np
    particles(i) = particles(i-1)+dp;
end
end

%% fuctions for mapping coordinates to one-dimension map
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% convert locations on map to particles
function [route_map_particle] = convertMaps(orig_route_maps)
n = length(orig_route_maps);
route_map_particle = cell(n,1);
for i=1:n
    route_map = orig_route_maps{i};
    route_map_particle{i} = convertSingleRoute(route_map);
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% convert route locations on map to particles
function [map_particles] = convertSingleRoute(map)
% compute distances
d = zeros(length(map),1);
for i = 2:length(map)
%     dis = sqrt((map(i,1) - map(i-1,1)).^2 + (map(i,2) - map(i-1,2)).^2);
    % calling haversine formula function
    dis = haversine([map(i,2) map(i,1)],[map(i-1,2) map(i-1,1)]);
    d(i) = dis + d(i-1);
end
idx = (1:1:length(map));
map_particles = [idx' d map];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% convert distance to coordinates (latitude, longitude)
% based on a existing route with positions
% search the nearest two particles that known the lati and long
% basicly these three poinits are in a straight line
% calculate the position
function [position] = convertDistance(distance, refRoute)
[i,j]=size(refRoute);
if isempty(distance) || distance < 0 || distance > refRoute(i, 2)
    position = [0,0];
    return;
end
[min_difference, array_position] = min(abs(refRoute(:,2)-distance));
if min_difference == 0
    % the point is just on the apex, return the position of apex point
    position = refRoute(array_position, 3:4);
else
    if refRoute(array_position,2) - distance > 0
        startPoint = refRoute(array_position - 1,:);
        endPoint = refRoute(array_position,:);
    else
        startPoint = refRoute(array_position,:);
        endPoint = refRoute(array_position + 1,:);
    end
    % calculate the point in the line segment
    lamda = (distance-startPoint(2))/(endPoint(2)-startPoint(2));
    x = lamda * (endPoint(3)-startPoint(3)) + startPoint(3);
    y = lamda * (endPoint(4)-startPoint(4)) + startPoint(4);
    position = [x, y];
end
end

%% functions for GPS measurement
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get the data from GPS
function [gps] = getGPSReading(k)
global gps_data;
global interval;
offset = 500;
index = interval * k + offset;
% simulate loss of GPS
lossRange = [interval+offset + 50,interval+offset + 50];
if index > 0 && index < length(gps_data)
    if index > lossRange(1,1) && index < lossRange(1,2)
        gps = [];
    else
        gps = gps_data(index,:);
    end
else
    gps = [];
end
plotGPS(gps);
end

%% fuctions for graph
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% draw the particles
function plotWeightedParticles(particles, p_w, refRoute)
ps = [];
pw = [];
for i = 1:length(particles)
    p = convertParticles(particles{i}, refRoute{i});
    ps = [ps ; p];
    pw = [pw; p_w{i}];
end
plotWeightedParticlesSingleRoute(ps, pw);
pause(0.1);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% draw the particles with weight for single route
function plotWeightedParticlesSingleRoute(positions, weights)
hold on;
% length(positions)
% graph reference that is used for drawing different color of particles
global p_G;
if isempty(p_G)
    p_G = [line, line, line, line, line, line];
end

n = (max(weights) - min(weights))/7;
d_p_0 = positions(weights <= n,:);
d_p_1 = positions(weights > 2*n,:);
d_p_2 = positions(weights > 3*n,:);
d_p_3 = positions(weights > 4*n,:);
d_p_4 = positions(weights > 5*n,:);
d_p_5 = positions(weights > 6*n,:);

c_0 = [0.8,0.8,0.8];
c_1 = [0.7,0.7,0.7];
c_2 = [0.5,0.5,0.5];
c_3 = [0.3,0.3,0.3];
c_4 = [0.2,0.2,0.2];
c_5 = [1,0,1];

set(p_G(1), 'XData',d_p_0(:,1),'YData',d_p_0(:,2),'Color',c_0,'LineStyle','.');
set(p_G(2), 'XData',d_p_1(:,1),'YData',d_p_1(:,2),'Color',c_1,'LineStyle','.');
set(p_G(3), 'XData',d_p_2(:,1),'YData',d_p_2(:,2),'Color',c_2,'LineStyle','.');
set(p_G(4), 'XData',d_p_3(:,1),'YData',d_p_3(:,2),'Color',c_3,'LineStyle','.');
set(p_G(5), 'XData',d_p_4(:,1),'YData',d_p_4(:,2),'Color',c_4,'LineStyle','.');
set(p_G(6), 'XData',d_p_5(:,1),'YData',d_p_5(:,2),'Color',c_5,'LineStyle','.');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% draw the bus maps based on stops
function plotMap(maps)
map1 = maps{1};
map2 = maps{2};
plot(map1(:,1),map1(:,2),'g.-');
plot(map2(:,1),map2(:,2),'b.-');
title('Two bus routes in Dublin area');
xlabel('Longitude');
ylabel('Latitude');
legend('Bus 39a','Bus 145');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% draw the data from GPS
function plotGPS(gps)
if ~isempty(gps)
plot(gps(1),gps(2),'r*');
end
end
